﻿#include "kinematics.h"
#include <cmath>
#include <QDebug>

namespace CIRT_LBR {
Transform::Transform()
{
    nx = 1; ox = 0; ax = 0;
    ny = 0; oy = 1; ay = 0;
    nz = 0; oz = 0; az = 1;
    px = 0; py = 0; pz = 0;
}

Transform::Transform(double _nx, double _ny, double _nz, double _ox, double _oy, double _oz, double _ax, double _ay, double _az, double _px, double _py, double _pz)
    :nx(_nx),ny(_ny),nz(_nz),
      ox(_ox),oy(_oy),oz(_oz),
      ax(_ax),ay(_ay),az(_az),
      px(_px),py(_py),pz(_pz)
{

}

Transform::Transform(const Transform &T)
{
    nx = T.nx; ny = T.ny; nz = T.nz;
    ox = T.ox; oy = T.oy; oz = T.oz;
    ax = T.ax; ay = T.ay; az = T.az;
    px = T.px; py = T.py; pz = T.pz;
}

Transform::Transform(const Transformation* T)
{ 
    nx = T->rotation[0][0]; ox = T->rotation[0][1]; ax = T->rotation[0][2];
    ny = T->rotation[1][0]; oy = T->rotation[1][1]; ay = T->rotation[1][2];
    nz = T->rotation[2][0]; oz = T->rotation[2][1]; az = T->rotation[2][2];
    px = T->translation[0]; py = T->translation[1]; pz = T->translation[2];
}

Transform::Transform(const Eigen::Matrix4d& T)
{
    nx = T(0, 0);
    ny = T(1, 0);
    nz = T(2, 0);
    ox = T(0, 1);
    oy = T(1, 1);
    oz = T(2, 1);
    ax = T(0, 2);
    ay = T(1, 2);
    az = T(2, 2);
    px = T(0, 3);
    py = T(1, 3);
    pz = T(2, 3);
}

Transform::Transform(const QVector<double> pos)
{   
    assert(pos.size() == 6);
    px = pos.at(0) / 1000.0;
    py = pos.at(1) / 1000.0;
    pz = pos.at(2) / 1000.0;
    double a=pos.at(3)*DEG2RAD, b=pos.at(4)*DEG2RAD,c = pos.at(5)*DEG2RAD;
    double cosyaw= cos(c);
    double sinyaw= sin(c);
    double cospitch= cos(b);
    double sinpitch= sin(b);
    double cosroll= cos(a);
    double sinroll= sin(a);
    nx = cosyaw * cospitch;
    ox = cosyaw * sinpitch * sinroll - sinyaw * cosroll;
    ax = cosyaw * sinpitch * cosroll + sinyaw * sinroll;
    ny = sinyaw * cospitch;
    oy = sinyaw * sinpitch * sinroll + cosyaw * cosroll;
    ay = sinyaw * sinpitch * cosroll - cosyaw * sinroll;
    nz = -sinpitch;
    oz = cospitch * sinroll;
    az = cospitch * cosroll;
//    Eigen::Vector3d eulerAngle(pos.at(5)*DEG2RAD, pos.at(4) * DEG2RAD, pos.at(3) * DEG2RAD);
//    Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(0), Eigen::Vector3d::UnitX()));
//    Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1), Eigen::Vector3d::UnitY()));
//    Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(2), Eigen::Vector3d::UnitZ()));
//    Eigen::Matrix3d rotation_matrix;
//    rotation_matrix = yawAngle * pitchAngle * rollAngle;
//    nx = rotation_matrix(0, 0);
//    ny = rotation_matrix(1, 0);
//    nz = rotation_matrix(2, 0);
//    ox = rotation_matrix(0, 1);
//    oy = rotation_matrix(1, 1);
//    oz = rotation_matrix(2, 1);
//    ax = rotation_matrix(0, 2);
//    ay = rotation_matrix(1, 2);
//    az = rotation_matrix(2, 2);
}

Transform& Transform::operator=(const Transform &T)
{
    nx = T.nx; ny = T.ny; nz = T.nz;
    ox = T.ox; oy = T.oy; oz = T.oz;
    ax = T.ax; ay = T.ay; az = T.az;
    px = T.px; py = T.py; pz = T.pz;
    return *this;
}

Transform& Transform::operator-=(const Transform &T)
{
    nx -= T.nx; ny -= T.ny; nz -= T.nz;
    ox -= T.ox; oy -= T.oy; oz -= T.oz;
    ax -= T.ax; ay -= T.ay; az -= T.az;
    px -= T.px; py -= T.py; pz -= T.pz;
    return *this;
}

double Transform::abs()
{
    double result = 0;
    result = std::fabs(nx)+std::fabs(ny)+std::fabs(nz)+
            std::abs(ox)+std::abs(oy)+std::fabs(oz)+
            std::abs(ax)+std::abs(ay)+std::fabs(az)+
            std::abs(px)+std::abs(py)+std::abs(pz);
    return result;
}

Transform Transform::operator*(const Transform &T)
{
    double a11,a21,a31,a12,a22,a32,a13,a23,a33,a14,a24,a34;
    a11 = nx*T.nx+ox*T.ny+ax*T.nz; a12 = nx*T.ox+ox*T.oy+ax*T.oz; a13 = nx*T.ax+ox*T.ay+ax*T.az;
    a21 = ny*T.nx+oy*T.ny+ay*T.nz; a22 = ny*T.ox+oy*T.oy+ay*T.oz; a23 = ny*T.ax+oy*T.ay+ay*T.az;
    a31 = nz*T.nx+oz*T.ny+az*T.nz; a32 = nz*T.ox+oz*T.oy+az*T.oz; a33 = nz*T.ax+oz*T.ay+az*T.az;
    a14 = nx*T.px+ox*T.py+ax*T.pz+px;
    a24 = ny*T.px+oy*T.py+ay*T.pz+py;
    a34 = nz*T.px+oz*T.py+az*T.pz+pz;
    return Transform(a11,a21,a31,a12,a22,a32,a13,a23,a33,a14,a24,a34);
}

Transformation* Transform::totransformation()
{
    Transformation* T = new Transformation();
    T->rotation[0][0] = nx; T->rotation[0][1] = ox; T->rotation[0][2] = ax;
    T->rotation[1][0] = ny; T->rotation[1][1] = oy; T->rotation[1][2] = ay;
    T->rotation[2][0] = nz; T->rotation[2][1] = oz; T->rotation[2][2] = az;
    T->translation[0] = px; T->translation[1] = py; T->translation[2] = pz;
    return T;
}

Transform Transform::inversed()
{
    Eigen::Matrix4d T;
    T.setIdentity();
    T(0, 0)=nx;
    T(1, 0)=ny;
    T(2, 0)=nz;
    T(0, 1)=ox;
    T(1, 1)=oy;
    T(2, 1)=oz;
    T(0, 2)=ax;
    T(1, 2)=ay;
    T(2, 2)=az;
    T(0, 3)=px;
    T(1, 3)=py;
    T(2, 3)=pz;
    return Transform(T.inverse());
}

QVector<double> Transform::toVector()
{
    Eigen::Matrix4d T;
    T(0, 0) = nx;
    T(1, 0) = ny;
    T(2, 0) = nz;
    T(0, 1) = ox;
    T(1, 1) = oy;
    T(2, 1) = oz;
    T(0, 2) = ax;
    T(1, 2) = ay;
    T(2, 2) = az;
    T(0, 3) = px;
    T(1, 3) = py;
    T(2, 3) = pz;
    Eigen::Matrix3d mtr;
    mtr = T.block<3, 3>(0, 0);
    Eigen::Vector3d el = mtr.eulerAngles(2, 1, 0);
    QVector<double> pos;
    pos << T(0, 3)*1000 << T(1, 3)*1000 << T(2, 3)*1000 << el.x() * RAD2DEG << el.y() * RAD2DEG << el.z() * RAD2DEG;
    return pos;
}

void Transform::toeulerangle()
{
    double trace = nx+oy+az; //matrix.m[0][0] + matrix.m[1][1] + matrix.m[2][2];
    double pitch, yaw, roll,w,x,y,z;
    if (trace > 0) {
        pitch = acos(az / trace);//matrix.m[2][2] / trace
        yaw = atan2(oz,nz);//(matrix.m[1][2], matrix.m[0][2]);
        roll = atan2(ax,ay);//(matrix.m[2][0], matrix.m[2][1]);
        w = atan2(ay-oz, ox - ny) * 180/PI; // in degrees
        x = atan2(-ax, az) * 180/PI; // in degrees
        y = atan2(nz - ax, ay - oz) * 180/PI; // in degrees
        z = atan2(ox + ny, oy + nx) * 180/PI; // in degrees
    } else if((nx>oy)&&(nx>az)) {// ((matrix.m[0][0] > matrix.m[1][1]) && (matrix.m[0][0] > matrix.m[2][2])) {
        pitch = acos(nx/trace);//(matrix.m[0][0] / trace);
        yaw = atan2(-ox,oy);//(-matrix.m[1][0], matrix.m[1][1]);
        roll = atan2(ay,az);//(matrix.m[2][1], matrix.m[2][2]);
    } else if (ny>az) { // (matrix.m[1][1] > matrix.m[2][2]) {
        pitch = acos(oy/trace);//(matrix.m[1][1] / trace);
        yaw = atan2(ny,nx);//(matrix.m[0][1], matrix.m[0][0]);
        roll = atan2(-ax,az);//(-matrix.m[2][0], matrix.m[2][2]);
    } else {
        pitch = acos(az/trace); //(matrix.m[2][2] / trace);
        yaw = atan2(nz,nx);//(matrix.m[0][2], matrix.m[0][0]);
        roll = atan2(ox,oy);//(matrix.m[1][0], matrix.m[1][1]);
    }


    qDebug()<<"rpy is:["<<roll * 180/PI<<pitch * 180/PI<<yaw * 180/PI<<"]"<<w<<x<<y<<z;


    x = atan2(ay, az);
    y = atan2(-ax, sqrt(ay * ay + az * az));
    z = atan2(ox, nx);

    qDebug()<<x* 180/PI<<y* 180/PI<<z* 180/PI;
//    printf("rpy is:[ ");
//    printf("%lf ",roll * 180/PI);
//    printf("%lf ",pitch * 180/PI);
//    printf("%lf ",yaw * 180/PI);
//    printf("]\n");
    //eulerAngle.pitch = pitch * 180 / M_PI;  // 将弧度转换为角度
    //eulerAngle.yaw = yaw * 180 / M_PI;  // 将弧度转换为角度
    //eulerAngle.roll = roll * 180 / M_PI;  // 将弧度转换为角度
}

std::ostream & operator<<(std::ostream &out, const Transform &T)
{
    out<<T.nx<<" "<<T.ox<<" "<<T.ax<<" "<<T.px<<"\n"
      <<T.ny<<" "<<T.oy<<" "<<T.ay<<" "<<T.py<<"\n"
     <<T.nz<<" "<<T.oz<<" "<<T.az<<" "<<T.pz<<"\n";
    return out;
}

Transform LBR_fk(const QVector<double> &Q)
{
    double d1,a2,a3,d4,d5,d6;
    // parameters
    d1 = 0.160;
    a2 = 0.7;
    a3 = 0.674;
    d4 = 0.169;
    d5 = 0.126;
    d6 = 0.107;

    double q1,q2,q3,q4,q5,q6;
    double offset[6] = {180,90,0,90,0,-180 };//{ -90,90,0,90,0,-90 };
    q1 = Q[0]+offset[0]; q2 = Q[1] + offset[1]; q3 = Q[2] + offset[2]; q4 = Q[3] + offset[3]; q5 = Q[4] + offset[4]; q6 = Q[5] + offset[5];
        q1 *= DEG2RAD; q2 *= DEG2RAD; q3 *= DEG2RAD;
    q4 *= DEG2RAD; q5 *= DEG2RAD; q6 *= DEG2RAD;
    double sinq1,sinq2,sinq5,sinq6,sinq23,sinq234;
    double cosq1,cosq2,cosq5,cosq6,cosq23,cosq234;
    sinq1 = sin(q1); cosq1 = cos(q1);
    sinq2 = sin(q2); cosq2 = cos(q2);
    sinq5 = sin(q5); cosq5 = cos(q5);
    sinq6 = sin(q6); cosq6 = cos(q6);
    sinq23 = sin(q2+q3); cosq23 = cos(q2+q3);
    sinq234 = sin(q2+q3+q4); cosq234 = cos(q2+q3+q4);

    double nx,ny,nz,ox,oy,oz,ax,ay,az,px,py,pz;
    nx = cosq1*cosq234*cosq5+sinq1*sinq5;
    ny = sinq1*cosq234*cosq5-cosq1*sinq5;
    nz = sinq234*cosq5;
    ox = -cosq1*cosq234*sinq5+sinq1*cosq5;
    oy = -sinq1*cosq234*sinq5-cosq1*cosq5;
    oz = -sinq234*sinq5;
    ax = cosq1*sinq234;
    ay = sinq1*sinq234;
    az = -cosq234;
    px = cosq1*sinq234*d5+cosq1*cosq23*a3+sinq1*d4+cosq1*cosq2*a2;
    py = sinq1*sinq234*d5+sinq1*cosq23*a3-cosq1*d4+sinq1*cosq2*a2;
    pz = -cosq234*d5+sinq23*a3+sinq2*a2+d1;

    double a11,a21,a31,a12,a22,a32,a13,a23,a33,a14,a24,a34;
    a11 = nx*cosq6-ax*sinq6;
    a21 = ny*cosq6-ay*sinq6;
    a31 = nz*cosq6-az*sinq6;
    a12 = -nx*sinq6-ax*cosq6;
    a22 = -ny*sinq6-ay*cosq6;
    a32 =  -nz*sinq6-az*cosq6;
    a13 = ox;
    a23 = oy;
    a33 = oz;
    a14 = d6*ox+px;
    a24 = d6*oy+py;
    a34 = d6*oz+pz;

    return Transform(a11,a21,a31,a12,a22,a32,a13,a23,a33,a14,a24,a34);
}

void LBR_fk(const VTYPE Q[6], Transformation* T)
{
    double d1, a2, a3, d4, d5, d6;
    // parameters
    d1 = 0.160;
    a2 = 0.7;
    a3 = 0.674;
    d4 = 0.169;
    d5 = 0.126;
    d6 = 0.107;
//    d1 = 0.1295;
//    a2 = 0.442;
//    a3 = 0.386;
//    d4 = 0.124;
//    d5 = 0.105;
//    d6 = 0.097;
    VTYPE q1, q2, q3, q4, q5, q6;
    VTYPE sinq1, sinq2, sinq5, sinq6, sinq23, sinq234;
    VTYPE cosq1, cosq2, cosq5, cosq6, cosq23, cosq234;
    VTYPE nx, ny, nz, ox, oy, oz, ax, ay, az, px, py, pz;
    //VTYPE a11,a21,a31,a12,a22,a32,a13,a23,a33,a14,a24,a34;
    double offset[6] = {180,90,0,90,0,-180};
    q1 = Q[0] + offset[0]; q2 = Q[1] + offset[1]; q3 = Q[2] + offset[2]; q4 = Q[3] + offset[3]; q5 = Q[4] + offset[4]; q6 = Q[5] + offset[5];
    q1 *= DEG2RAD; q2 *= DEG2RAD; q3 *= DEG2RAD;
    q4 *= DEG2RAD; q5 *= DEG2RAD; q6 *= DEG2RAD;
    sinq1 = sin(q1); cosq1 = cos(q1);
    sinq2 = sin(q2); cosq2 = cos(q2);
    sinq5 = sin(q5); cosq5 = cos(q5);
    sinq6 = sin(q6); cosq6 = cos(q6);
    sinq23 = sin(q2 + q3); cosq23 = cos(q2 + q3);
    sinq234 = sin(q2 + q3 + q4); cosq234 = cos(q2 + q3 + q4);

    nx = cosq1 * cosq234 * cosq5 + sinq1 * sinq5;
    ny = sinq1 * cosq234 * cosq5 - cosq1 * sinq5;
    nz = sinq234 * cosq5;
    ox = -cosq1 * cosq234 * sinq5 + sinq1 * cosq5;
    oy = -sinq1 * cosq234 * sinq5 - cosq1 * cosq5;
    oz = -sinq234 * sinq5;
    ax = cosq1 * sinq234;
    ay = sinq1 * sinq234;
    az = -cosq234;
    px = cosq1 * sinq234 * d5 + cosq1 * cosq23 * a3 + sinq1 * d4 + cosq1 * cosq2 * a2;
    py = sinq1 * sinq234 * d5 + sinq1 * cosq23 * a3 - cosq1 * d4 + sinq1 * cosq2 * a2;
    pz = -cosq234 * d5 + sinq23 * a3 + sinq2 * a2 + d1;

    T->rotation[0][0] = nx * cosq6 - ax * sinq6;
    T->rotation[1][0] = ny * cosq6 - ay * sinq6;
    T->rotation[2][0] = nz * cosq6 - az * sinq6;
    T->rotation[0][1] = -nx * sinq6 - ax * cosq6;
    T->rotation[1][1] = -ny * sinq6 - ay * cosq6;
    T->rotation[2][1] = -nz * sinq6 - az * cosq6;
    T->rotation[0][2] = ox;
    T->rotation[1][2] = oy;
    T->rotation[2][2] = oz;
    T->translation[0] = d6 * ox + px;
    T->translation[1] = d6 * oy + py;
    T->translation[2] = d6 * oz + pz;
}

double checkangle(double angle)
{
    double a=angle;
    if(a <= -360)
        a+=360;
    if(a >= 360)
        a-=360;
    if(fabs(angle)<1e-4)
        a=0;
    return a;
}
QVector<QVector<double> > LBR_ik(const Transform &T)
{
    double d1,a2,a3,d4,d5,d6;
    // parameters
//    d1 = 0.1295;
//    a2 = 0.442;
//    a3 = 0.386;
//    d4 = 0.124;
//    d5 = 0.105;
//    d6 = 0.097;
    d1 = 0.160;
    a2 = 0.7;
    a3 = 0.674;
    d4 = 0.169;
    d5 = 0.126;
    d6 = 0.107;
    double offset[6] = {180,90,0,90,0,-180 };
    double nx,ny,nz,ox,oy,oz,ax,ay,az,px,py,pz;
    nx = T.nx;
    ny = T.ny;
    nz = T.nz;
    ox = T.ox;
    oy = T.oy;
    oz = T.oz;
    ax = T.ax;
    ay = T.ay;
    az = T.az;

    px = T.px; py = T.py; pz = T.pz;

    QVector<QVector<double> > Q;
    QVector<double> q_last,q_current(6,0);
    double q1,q2,q3,q234,q4,q5,q6;
    double sinq1,sinq2,sinq3,sinq5,sinq6,sinq234;
    double cosq1,cosq2,cosq3,cosq5,cosq6,cosq234;

    // ---------- solve q1 ----------
    std::vector<std::pair<double, double> > q1_sin_cos_pair;
    q1_sin_cos_pair = phase_shift_eq(-px+d6*ax,py-d6*ay,-d4);
    //std::cout<<"q1 num: "<<q1_sin_cos_pair.size()<<std::endl;
    if(!q1_sin_cos_pair.size())
    {
        std::cout<<"no q1 exists"<<std::endl;
        return Q;
    }
    double h1,h2,h3,h4;
    h1 = a3+d5-a2;
    h2 = a2+d5-a3;
    h3 = a2+a3-d5;
    h4 = a2+a3+d5;

    for(unsigned int i=0;i<q1_sin_cos_pair.size();++i)
    {
        sinq1 = q1_sin_cos_pair[i].first;
        cosq1 = q1_sin_cos_pair[i].second;

        std::vector<std::pair<double, double> > q5_sin_cos_pair;
        // --------- solve q5 ---------
        cosq5 = (-d4+px*sinq1-py*cosq1)/d6;
        sinq5 = 0;
        if(std::fabs(std::fabs(cosq5)-1)>1e-6)
        {
           // std::cout<<"cosq5 not close to -1 or 1"<<std::endl;
            if(cosq5>1||cosq5<-1)
            {
                //std::cout<<"cosq5 exceeds range [-1, 1]"<<std::endl;
                //std::cout<<"no q5 exists"<<std::endl;
                continue;
            }
            q5_sin_cos_pair.push_back(std::make_pair(std::sqrt(1-cosq5*cosq5),cosq5));
            q5_sin_cos_pair.push_back(std::make_pair(-std::sqrt(1-cosq5*cosq5),cosq5));
        }
        else{
            q5_sin_cos_pair.push_back(std::make_pair(0,cosq5));
        }

        double p15x,p15z,p15;
        p15x = cosq1*(px-d6*ax)+sinq1*(py-d6*ay);
        p15z = pz-d1-d6*az;
        p15 = p15x*p15x + p15z*p15z;
        double alpha,beta,M,N;

        alpha = cosq1*nx+sinq1*ny;
        beta = cosq1*ox+sinq1*oy;
        M = p15x*alpha+p15z*nz;
        N = p15x*beta+p15z*oz;

        for(unsigned int j=0;j<q5_sin_cos_pair.size();++j)
        {
            // ------- solve q6 --------
            sinq5 = q5_sin_cos_pair[j].first;
            cosq5 = q5_sin_cos_pair[j].second;

            //std::cout<<"sinq5: "<<sinq5<<std::endl;

            std::vector<std::pair<double,double> > q6_sin_cos_pair;

            if(std::fabs(std::fabs(cosq5)-1)<1e-6) // joint 2,3,4,6 parallel
            {
                //std::cout<<"--- 2,3,4,6 joints paraller ---"<<std::endl;

                // ------ range check -------
                double delta_upper = p15-h4*h4;
                double delta_lower = p15-h1*h1;
                if(delta_upper>1e-6)
                {
                    //std::cout<<"delta_upper: "<<delta_upper<<std::endl;
                    //std::cout<<"no solution, strech out of range upper"<<std::endl;
                    continue;
                }

                if(delta_lower<-1e-6)
                {
                    //std::cout<<"delta_lower: "<<delta_lower<<std::endl;
                    //std::cout<<"no solution, strech out of range lower";
                    continue;
                }

                //                if(p15>h2*h2)
                //                {
                //                    if(p15<h3*h3) // ----- middle segment -----
                //                    {
                //                        std::cout<<"--- middle segment ---"<<std::endl;
                //                        q6_sin_cos_pair = phase_shift_eq(M/std::sqrt(p15),N/std::sqrt(p15),(std::sqrt(p15)-a2-a3+d5)/(a3-d5)+1);
                //                    }
                //                    else    // ----- right segment -----
                //                    {
                //                        std::cout<<"--- right segment ---"<<std::endl;
                //                        q6_sin_cos_pair.push_back(std::make_pair<double,double>(M/std::sqrt(p15),N/std::sqrt(p15)));
                //                    }

                //                }
                //                else     // ----- left segment -----
                //                {
                //                    std::cout<<"--- left segment ---"<<std::endl;
                //                    q6_sin_cos_pair.push_back(std::make_pair<double,double>(-M/std::sqrt(p15),-N/std::sqrt(p15)));
                //                }
                double sqrt_p15 = std::sqrt(p15);
                q6_sin_cos_pair = phase_shift_eq(M/sqrt_p15,N/sqrt_p15,-(sqrt_p15+a2-a3-d5)/a2+1);


            }
            else   // 2,3,4,6 joints not parallel
            {
                //std::cout<<"2,3,4,6, joints not parallel"<<std::endl;
                q6_sin_cos_pair.push_back(std::make_pair<double,double>(-(ox*sinq1-oy*cosq1)/sinq5,-(-nx*sinq1+ny*cosq1)/sinq5));
            }
            //std::cout<<"q6 num "<<q6_sin_cos_pair.size()<<std::endl;
            // solve q2
            for(unsigned int k=0;k<q6_sin_cos_pair.size();++k)
            {
                sinq6 = q6_sin_cos_pair[k].first;
                cosq6 = q6_sin_cos_pair[k].second;
                //std::cout<<"sinq6: "<<sinq6<<std::endl;
                //std::cout<<"cosq6: "<<cosq6<<std::endl;
                // solve q2
                sinq234 = -alpha*sinq6-beta*cosq6;
                cosq234 = nz*sinq6+oz*cosq6;
                double A,B;
                A = p15x-sinq234*d5;
                B = p15z+cosq234*d5;
                std::vector<std::pair<double,double> > q2_sin_cos_pair;
                q2_sin_cos_pair = phase_shift_eq(2*a2*B,2*a2*A,A*A+B*B+a2*a2-a3*a3);
                if(!q2_sin_cos_pair.size())
                {
                    //std::cout<<"no q2 exists"<<std::endl;
                    continue;
                }
                //std::cout<<"q2 num "<<q2_sin_cos_pair.size()<<std::endl;
                for(unsigned int l=0;l<q2_sin_cos_pair.size();++l)
                {
                    sinq2 = q2_sin_cos_pair[l].first;
                    cosq2 = q2_sin_cos_pair[l].second;
                    sinq3 = (B*cosq2-A*sinq2)/a3;
                    cosq3 = (A*A+B*B-a2*a2-a3*a3)/2/a2/a3;
                    q234 = std::atan2(sinq234,cosq234);
                    q2 = std::atan2(sinq2,cosq2);
                    q3 = std::atan2(sinq3,cosq3);
                    q4 = q234-q2-q3;
                    if(q4<-PI)
                    {
                        q4+=2*PI;
                    }
                    if(q4>PI)
                    {
                        q4-=2*PI;
                    }
                    q1 = std::atan2(sinq1,cosq1);
                    q5 = std::atan2(sinq5,cosq5);
                    q6 = std::atan2(sinq6,cosq6);
                    q_current[0]=q1*180/PI - offset[0];
                    q_current[1]=q2*180/PI - offset[1];
                    q_current[2]=q3*180/PI - offset[2];
                    q_current[3]=q4*180/PI - offset[3];
                    q_current[4]=q5*180/PI - offset[4];
                    q_current[5]=q6*180/PI - offset[5];
                    for(int i=0;i<6;++i)
                    {
                        q_current[i]=checkangle(q_current[i]);
                    }
                    if(q_last.size())
                    {
                        if(!isequal(q_last,q_current))
                        {
                            Q.push_back(q_current);
                            q_last = q_current;
                            //std::cout<<"got one solution"<<std::endl;
                        }
                    }
                    else
                    {
                        Q.push_back(q_current);
                        q_last = q_current;
                        //std::cout<<"got one solution"<<std::endl;
                    }
                }
            }
        }
    }
    //std::cout<<"LBR ik done"<<std::endl;
    return Q;
}

void LBR_ik(const Transformation* T, unsigned int* solutionnum, VTYPE inversesolutions[8][6])
{
    double d1, a2, a3, d4, d5, d6;
    // parameters
//    d1 = 0.1295;
//    a2 = 0.442;
//    a3 = 0.386;
//    d4 = 0.124;
//    d5 = 0.105;
//    d6 = 0.097;
    d1 = 0.160;
    a2 = 0.7;
    a3 = 0.674;
    d4 = 0.169;
    d5 = 0.126;
    d6 = 0.107;

    unsigned int currentsolutionnum = 0;
    // preperations for inverse kinematics
    VTYPE nx, ny, nz, ox, oy, oz, ax, ay, az, px, py, pz;
    nx = T->rotation[0][0]; ny = T->rotation[1][0]; nz = T->rotation[2][0];
    ox = T->rotation[0][1]; oy = T->rotation[1][1]; oz = T->rotation[2][1];
    ax = T->rotation[0][2]; ay = T->rotation[1][2]; az = T->rotation[2][2];
    px = T->translation[0]; py = T->translation[1]; pz = T->translation[2];
    double offset[6] = {180,90,0,90,0,-180};
    VTYPE solution[6] = { 0,0,0,0,0,0 };
    VTYPE q2, q3, q234, q4;
    VTYPE sinq1, sinq2, sinq3, sinq5, sinq6, sinq234;
    VTYPE cosq1, cosq2, cosq3, cosq5, cosq6, cosq234;

    // ---------- solve q1 ----------
    VTYPE q1_sin_cos_pair[2][2];
    unsigned char q1_num = phase_shift_eq(q1_sin_cos_pair, -px + d6 * ax, py - d6 * ay, -d4);

    if (!q1_num)
    {
        printf("no q1 solution\n");
        *solutionnum = 0;
        return; // no inverse solution
    }

    //printf("q1 num:%d\n",q1_num);

    VTYPE h1, h4;
    h1 = a3 + d5 - a2;
    h4 = a2 + a3 + d5;

    for (unsigned char i = 0; i < q1_num; ++i)
    {
        sinq1 = q1_sin_cos_pair[i][0];
        cosq1 = q1_sin_cos_pair[i][1];

        // --------- solve q5 ---------
        VTYPE q5_sin_cos_pair[2][2];
        unsigned char q5_num;
        cosq5 = (-d4 + px * sinq1 - py * cosq1) / d6;
        if (fabs(fabs(cosq5) - 1) > EPSR)
        {
            if (cosq5 > 1 || cosq5 < -1)
            {
                continue;
            }
            sinq5 = sqrt(1 - cosq5 * cosq5);
            q5_sin_cos_pair[0][0] = sinq5;
            q5_sin_cos_pair[0][1] = cosq5;
            q5_sin_cos_pair[1][0] = -sinq5;
            q5_sin_cos_pair[1][1] = cosq5;
            q5_num = 2;
        }
        else {
            q5_sin_cos_pair[0][0] = 0;
            q5_sin_cos_pair[0][1] = cosq5;
            q5_num = 1;
        }

        VTYPE p15x, p15z, p15, sqrt_p15;
        p15x = cosq1 * (px - d6 * ax) + sinq1 * (py - d6 * ay);
        p15z = pz - d1 - d6 * az;
        p15 = p15x * p15x + p15z * p15z;
        sqrt_p15 = sqrt(p15);
        VTYPE alpha, beta, M, N;

        alpha = cosq1 * nx + sinq1 * ny;
        beta = cosq1 * ox + sinq1 * oy;
        M = p15x * alpha + p15z * nz;
        N = p15x * beta + p15z * oz;

        //printf("q5 num:%d\n",q5_num);

        for (unsigned char j = 0; j < q5_num; ++j)
        {
            sinq5 = q5_sin_cos_pair[j][0];
            cosq5 = q5_sin_cos_pair[j][1];
            // --------- solve q6 ---------
            VTYPE q6_sin_cos_pair[2][2];
            unsigned char q6_num;
            if (fabs(sinq5) < EPSR) // joint 2,3,4,6 parallel
            {
                // ------ range check -------
                VTYPE delta_upper = p15 - h4 * h4;
                VTYPE delta_lower = p15 - h1 * h1;
                if (delta_upper > EPSR || delta_lower < -EPSR)
                {
                    continue;
                }
                q6_num = phase_shift_eq(q6_sin_cos_pair, M / sqrt_p15, N / sqrt_p15, -(sqrt_p15 + a2 - a3 - d5) / a2 + 1);

            }
            else  // joint 2,3,4,6 not parallel
            {
                q6_sin_cos_pair[0][0] = -(ox * sinq1 - oy * cosq1) / sinq5;
                q6_sin_cos_pair[0][1] = -(-nx * sinq1 + ny * cosq1) / sinq5;
                q6_num = 1;
            }

            //printf("q6 num:%d\n",q6_num);

            for (unsigned char k = 0; k < q6_num; ++k)
            {
                sinq6 = q6_sin_cos_pair[k][0];
                cosq6 = q6_sin_cos_pair[k][1];
                // ----- solve q2 -----
                sinq234 = -alpha * sinq6 - beta * cosq6;
                cosq234 = nz * sinq6 + oz * cosq6;
                VTYPE A, B;
                A = p15x - sinq234 * d5;
                B = p15z + cosq234 * d5;
                VTYPE q2_sin_cos_pair[2][2];
                unsigned char q2_num;
                q2_num = phase_shift_eq(q2_sin_cos_pair, 2 * a2 * B, 2 * a2 * A, A * A + B * B + a2 * a2 - a3 * a3);
                if (!q2_num)
                {
                    continue;
                }

                //printf("q2 num:%d\n",q2_num);

                for (unsigned char l = 0; l < q2_num; ++l)
                {
                    sinq2 = q2_sin_cos_pair[l][0];
                    cosq2 = q2_sin_cos_pair[l][1];
                    // ----- solve q3 -----
                    sinq3 = (B * cosq2 - A * sinq2) / a3;
                    cosq3 = (A * A + B * B - a2 * a2 - a3 * a3) / 2 / a2 / a3;
                    q234 = atan2(sinq234, cosq234);
                    q2 = atan2(sinq2, cosq2);
                    q3 = atan2(sinq3, cosq3);
                    // ----- solve q4 -----
                    q4 = q234 - q2 - q3;
                    if (q4 < -M_PI)
                    {
                        q4 += 2 * M_PI;
                    }
                    if (q4 > M_PI)
                    {
                        q4 -= 2 * M_PI;
                    }
                    //printf("q4: %lf\n",q4*RAD2DEG);
                    solution[0] = atan2(sinq1, cosq1) * RAD2DEG-offset[0];
                    solution[1] = q2 * RAD2DEG - offset[1];
                    solution[2] = q3 * RAD2DEG - offset[2];
                    solution[3] = q4 * RAD2DEG - offset[3];
                    solution[4] = atan2(sinq5, cosq5) * RAD2DEG - offset[4];
                    solution[5] = atan2(sinq6, cosq6) * RAD2DEG - offset[5];

                    for(int i=0;i<6;++i)
                    {
                        solution[i]=checkangle(solution[i]);
                    }

                    for (unsigned int m = 0; m < JOINT_NUM; ++m)
                    {
                        inversesolutions[currentsolutionnum][m] = solution[m];
                    }
                    currentsolutionnum++;
                }
            }

        }

    }
    * solutionnum = currentsolutionnum;
}

std::vector<std::pair<double, double> > phase_shift_eq(const double& a,const double& b, const double& c)
{
    std::vector<std::pair<double, double> > sin_cos_pair;
    double denominator = a*a+b*b;
    double delta = denominator-c*c;

    if(std::abs(delta)<1e-6)
    {
        double sinx = a*c/denominator;
        double cosx = b*c/denominator;
        sincosround(sinx); sincosround(cosx);
        sin_cos_pair.push_back(std::make_pair(sinx,cosx));
        return sin_cos_pair;
    }

    if(delta<0)
    {
        return sin_cos_pair;
    }

    double sinx1,cosx1,sinx2,cosx2;
    sinx1 = (a*c+b*std::sqrt(delta))/denominator;
    cosx1 = (b*c-a*std::sqrt(delta))/denominator;
    sinx2 = (a*c-b*std::sqrt(delta))/denominator;
    cosx2 = (b*c+a*std::sqrt(delta))/denominator;
    sincosround(sinx1); sincosround(cosx1); sincosround(sinx2); sincosround(cosx2);
    sin_cos_pair.push_back(std::make_pair(sinx1,cosx1));
    sin_cos_pair.push_back(std::make_pair(sinx2,cosx2));

    return sin_cos_pair;
}

unsigned char phase_shift_eq(VTYPE sin_cos_pairs[2][2], const VTYPE a, const VTYPE b, const VTYPE c)
{
    VTYPE denominator = a * a + b * b;
    VTYPE delta = denominator - c * c;
    if (fabs(delta) < EPSR)
    {
        VTYPE sinx = a * c / denominator;
        VTYPE cosx = b * c / denominator;
        sincosround(&sinx);
        sincosround(&cosx);
        sin_cos_pairs[0][0] = sinx;
        sin_cos_pairs[0][1] = cosx;
        //printf("one solution\n");
        return 1;
    }

    if (delta < 0)
    {
        if (fabs(a - c) < EPSR)
        {
            sin_cos_pairs[0][0] = 1;
            sin_cos_pairs[0][1] = 0;
            return 1;
        }
        if (fabs(a + c) < EPSR)
        {
            sin_cos_pairs[0][0] = -1;
            sin_cos_pairs[0][1] = 0;
            return 1;
        }
        if (fabs(b - c) < EPSR)
        {
            sin_cos_pairs[0][0] = 0;
            sin_cos_pairs[0][1] = 1;
            return 1;
        }
        if (fabs(b + c) < EPSR)
        {
            sin_cos_pairs[0][0] = 0;
            sin_cos_pairs[0][1] = -1;
            return 1;
        }
        //printf("no solution\n");
        return 0;
    }

    VTYPE sqrt_delta = sqrt(delta);
    VTYPE sinx1, cosx1, sinx2, cosx2;
    sinx1 = (a * c + b * sqrt_delta) / denominator;
    cosx1 = (b * c - a * sqrt_delta) / denominator;
    sinx2 = (a * c - b * sqrt_delta) / denominator;
    cosx2 = (b * c + a * sqrt_delta) / denominator;
    sincosround(&sinx1); sincosround(&cosx1); sincosround(&sinx2); sincosround(&cosx2);
    sin_cos_pairs[0][0] = sinx1;
    sin_cos_pairs[0][1] = cosx1;
    sin_cos_pairs[1][0] = sinx2;
    sin_cos_pairs[1][1] = cosx2;
    //printf("two solution\n");
    return 2;
}

bool isequal(const QVector<double> &a, const QVector<double> &b)
{
    double sum;
    for(unsigned int i=0;i<a.size();++i)
    {
        sum+=std::abs(a[i]-b[i]);
    }
    if(sum<1e-6)
    {
        return true;
    }

    return false;
}

void sincosround(double &value)
{
    if(std::fabs(value-1)<1e-6)
    {
        value = 1;
        return;
    }
    if(std::fabs(value+1)<1e-6)
    {
        value = -1;
        return;
    }
    if(std::fabs(value)<1e-6)
    {
        value = 0;
        return;
    }
}

void sincosround(VTYPE* value)
{
    if (fabs(*value - 1) < EPSR)
    {
        *value = 1;
        //printf("close 1\n");
        return;
    }
    if (fabs(*value + 1) < EPSR)
    {
        *value = -1;
        //printf("close -1\n");
        return;
    }
    if (fabs(*value) < EPSR)
    {
        *value = 0;
        //printf("close 0\n");
        return;
    }
}

double maxdistance(const QVector<double>& joint1, const QVector<double>& joint2)//joint2，joint1 中为角度
{
    double dist = 0;
    for (int i = 0; i < joint1.size() - 1; ++i)
    {
        double delta = joint1.at(i) - joint2.at(i);
        if (delta >= 360)
        {
            delta -= 360;
        }
        else if (delta <= -360)
        {
            delta += 360;
        }
        dist += qAbs(delta);
    }
    return dist;
}

QVector<float> nearstsloution(const QVector<QVector<double> > sluts, const QVector<double> current, double tolrance)
{
    QVector<float> slot;
    if(sluts.size()==0)
    {
        qDebug()<< "no slutions found about ";
        return {};
    }
    else
    {
        //qDebug()<<"select sloution............"<<sluts;
        double del=maxdistance(current,sluts.at(0));

        int index=0;
        for(int i=1;i<sluts.size();++i)
        {
            double maxdist=maxdistance(current,sluts.at(i));
            //qDebug()<<"maxdist "<<i<<"=="<<maxdist;
            if(maxdist<del)
            {
                del=maxdist;
                index=i;
            }

        }
        //qDebug()<<"last index =="<<index<<del;
        if(del>tolrance)
            return {};
        for(int j=0;j<6;++j)
        {
            slot.push_back(sluts.at(index).at(j));
        }
    }
    return slot;
}

VTYPE caldistance(const VTYPE Q1[6], const VTYPE Q2[6])
{
    VTYPE delta = fabs(Q1[0] - Q2[0]);
    VTYPE d = delta;
    for (unsigned int i = 1; i < JOINT_NUM; ++i)
    {
        delta = fabs(Q1[i] - Q2[i]);
        if (delta > d)
        {
            d = delta;
        }
    }
    return d;
}

}
